/*
 * @Author: wangyingjie 2778809626@qq.com
 * @Date: 2025-07-14 15:36:05
 * @LastEditors: wangyingjie 2778809626@qq.com
 * @LastEditTime: 2025-07-22 14:47:56
 * @FilePath: /two_wheeled_car/src/bot_control/src/sensor_reader.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <vector>
#include <cmath>
#include <algorithm>
#include <memory>
#include <queue>
#include <mutex>
#include <deque>

// 参数配置
constexpr double MAX_LINEAR_SPEED = 0.9;    // 最大线速度 (m/s)
constexpr double MAX_ANGULAR_SPEED = 5.0;   // 最大角速度 (rad/s)
constexpr double MAX_LINEAR_ACCEL = 0.9;    // 最大线加速度 (m/s²)
constexpr double MAX_ANGULAR_ACCEL = 5;   // 最大角加速度 (rad/s²)
constexpr double ROBOT_RADIUS = 1.2;        // 船舶半径 (包括安全余量)
constexpr double OBSTACLE_INFLATION = 0.5;  // 障碍物膨胀半径
constexpr double DT = 0.1;                  // 控制周期 (100ms)
constexpr double PREDICT_TIME = 4.0;        // 轨迹预测时间 (s)
constexpr double GOAL_TOLERANCE = 1.0;      // 目标点容差 (m)
constexpr double LOOKAHEAD_DIST = 5.0;      // 前瞻距离 (m)
constexpr double PATH_FOLLOWING_DIST = 3.0; // 路径点切换距离 (m)

// 目标函数权重
constexpr double ALPHA = 0.5;   // 降低朝向权重
constexpr double BETA = 3.0;    // 显著提高速度权重
constexpr double GAMMA = 0.1;   // 最小化安全权重
constexpr double DELTA = 0.0;   // 禁用水流影响

class DWAPlanner : public rclcpp::Node
{
public:
    DWAPlanner() : Node("dwa_planner")
    {
        // 初始化状态
        flow_x_ = 0.0;
        flow_y_ = 0.0;
        last_cmd_v_ = 0.0;
        last_cmd_w_ = 0.0;
        current_goal_index_ = -1;
        path_received_ = false;

        // 订阅雷达数据
        radar_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
            "/radar/scan", 10,
            std::bind(&DWAPlanner::radarCallback, this, std::placeholders::_1));

        // 订阅里程计（定位）
        odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10,
            std::bind(&DWAPlanner::odomCallback, this, std::placeholders::_1));

        // 订阅IMU
        imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
            "/imu", 10,
            std::bind(&DWAPlanner::imuCallback, this, std::placeholders::_1));

        // 订阅路径
        path_sub_ = create_subscription<nav_msgs::msg::Path>(
            "/path", 10,
            std::bind(&DWAPlanner::pathCallback, this, std::placeholders::_1));

        // 发布速度指令
        cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

        // 发布当前目标点（可视化）
        goal_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("/current_goal", 10);

        // 定时器，控制DWA运行频率
        timer_ = create_wall_timer(
            std::chrono::milliseconds(static_cast<int>(DT * 1000)),
            std::bind(&DWAPlanner::dwaControl, this));
        
        forcast_path_pub_ = create_publisher<nav_msgs::msg::Path>("/predicted_path", 10);

        RCLCPP_INFO(get_logger(), "DWA Path Follower initialized. Waiting for path...");
    }

private:
    // 回调函数
    void radarCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(obs_mutex_);

        // 清空障碍物列表
        obstacles_.clear();

        // 雷达参数
        double angle_min = msg->angle_min;
        double angle_increment = msg->angle_increment;
        double range_min = msg->range_min;
        double range_max = msg->range_max;

        // 动态阈值滤波 (简化版)
        double avg_range = 0.0;
        int valid_count = 0;
        for (auto range : msg->ranges)
        {
            if (range > range_min && range < range_max)
            {
                avg_range += range;
                valid_count++;
            }
        }
        if (valid_count > 0)
            avg_range /= valid_count;

        // 处理雷达数据
        for (size_t i = 0; i < msg->ranges.size(); ++i)
        {
            double range = msg->ranges[i];

            // 过滤无效数据和杂波
            if (range < range_min || range > range_max || range > avg_range * 1.5)
            {
                continue;
            }

            double angle = angle_min + i * angle_increment;

            // 雷达坐标系下的点
            double x_l = range * cos(angle);
            double y_l = range * sin(angle);

            // 转换到船体坐标系（假设雷达安装在船中心）
            // 然后转换到世界坐标系
            double x_w = current_x_ + x_l * cos(current_yaw_) - y_l * sin(current_yaw_);
            double y_w = current_y_ + x_l * sin(current_yaw_) + y_l * cos(current_yaw_);

            // 添加到障碍物列表
            obstacles_.push_back(std::make_pair(x_w, y_w));
        }

        RCLCPP_DEBUG(get_logger(), "Processed %zu obstacles", obstacles_.size());
    }

    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        // 更新位置
        current_x_ = msg->pose.pose.position.x;
        current_y_ = msg->pose.pose.position.y;

        // 更新速度
        current_vx_ = msg->twist.twist.linear.x;
        current_vy_ = msg->twist.twist.linear.y;
        current_v_ = sqrt(current_vx_ * current_vx_ + current_vy_ * current_vy_);

        // 更新航向角
        tf2::Quaternion q(
            msg->pose.pose.orientation.x,
            msg->pose.pose.orientation.y,
            msg->pose.pose.orientation.z,
            msg->pose.pose.orientation.w);
        tf2::Matrix3x3 m(q);
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);
        current_yaw_ = yaw;

        // 角速度
        current_w_ = msg->twist.twist.angular.z;
    }

    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        // 更新角速度
        current_w_ = msg->angular_velocity.z;
    }

    void pathCallback(const nav_msgs::msg::Path::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(path_mutex_);

        if (msg->poses.empty())
        {
            RCLCPP_WARN(get_logger(), "Received empty path!");
            return;
        }

        // 检查新路径是否与当前路径相同
        bool is_same_path = true;

        if (path_poses_.size() == msg->poses.size())
        {
            for (size_t i = 0; i < msg->poses.size(); ++i)
            {
                const auto &new_pose = msg->poses[i].pose;
                const auto &current_pose = path_poses_[i];

                // 比较位置和方向是否相同（使用一个小的阈值判断浮点误差）
                double dx = new_pose.position.x - current_pose.position.x;
                double dy = new_pose.position.y - current_pose.position.y;
                double dz = new_pose.position.z - current_pose.position.z;

                if (dx * dx + dy * dy + dz * dz > 1e-6) // 位置不同
                {
                    is_same_path = false;
                    break;
                }

                // 比较四元数是否相同
                double qw = new_pose.orientation.w - current_pose.orientation.w;
                double qx = new_pose.orientation.x - current_pose.orientation.x;
                double qy = new_pose.orientation.y - current_pose.orientation.y;
                double qz = new_pose.orientation.z - current_pose.orientation.z;

                if (qw * qw + qx * qx + qy * qy + qz * qz > 1e-6) // 方向不同
                {
                    is_same_path = false;
                    break;
                }
            }
        }
        else
        {
            is_same_path = false;
        }

        if (is_same_path)
        {
            RCLCPP_DEBUG(get_logger(), "Received the same path, skipping update.");
            return; // 路径相同，无需更新
        }

        // 清空当前路径并存储新路径
        path_poses_.clear();
        for (const auto &pose_stamped : msg->poses)
        {
            path_poses_.push_back(pose_stamped.pose);
        }

        // 重置目标点索引
        current_goal_index_ = 0;
        path_received_ = true;

        RCLCPP_INFO(get_logger(), "Received new path with %zu points", path_poses_.size());
    }

    // DWA核心控制函数
    void dwaControl()
    {
        // 检查是否收到路径
        if (!path_received_)
        {
            RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "No path received yet!");
            return;
        }

        // 检查是否完成所有路径点
        if (current_goal_index_ >= static_cast<int>(path_poses_.size()))
        {
            RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000, "All path points reached!");
            stopRobot();
            return;
        }

        // 1. 估算水流 (基于运动学模型)
        estimateFlow();

        // 2. 更新当前目标点
        updateCurrentGoal();

        // 3. 生成动态窗口
        double v_min, v_max, w_min, w_max;
        generateDynamicWindow(current_v_, current_w_, v_min, v_max, w_min, w_max);

        // 4. 采样速度空间并评估轨迹
        double best_score = -1e9;
        double best_v = 0.0;
        double best_w = 0.0;
        std::vector<geometry_msgs::msg::Pose> best_trajectory;
        bool valid_trajectory_found = false;

        // 速度采样分辨率
        const double v_step = 0.1;
        const double w_step = 0.1;

        bool any_valid_trajectory = false; // 是否生成了有效轨迹

        // 动态调整采样步长，确保至少采样一次
        double adjusted_v_step = v_step;
        if (v_max - v_min < v_step) {
            adjusted_v_step = (v_max - v_min) / 2.0; // 至少采样两次
            if (adjusted_v_step <= 0.0) {
                adjusted_v_step = v_step; // 如果窗口无效，使用默认步长
            }
        }

        for (double v = v_min; v <= v_max; v += adjusted_v_step)
        {
            for (double w = w_min; w <= w_max; w += w_step)
            {
                // 模拟轨迹
                std::vector<geometry_msgs::msg::Pose> trajectory = simulateTrajectory(v, w, DT, static_cast<int>(PREDICT_TIME / DT));

                // 发布轨迹用于可视化
                publishTrajectory(trajectory);

                // 计算目标函数得分
                double score = evaluateTrajectory(trajectory, v, w);

                // 更新最优轨迹
                if (score > best_score)
                {
                    best_score = score;
                    best_v = v;
                    best_w = w;
                    best_trajectory = trajectory;
                    any_valid_trajectory = true;
                }
            }
        }

        // 如果没有生成任何有效轨迹，强制采样中点
        if (!any_valid_trajectory)
        {
            double v = (v_min + v_max) / 2.0;
            double w = (w_min + w_max) / 2.0;

            std::vector<geometry_msgs::msg::Pose> trajectory = simulateTrajectory(v, w, DT, static_cast<int>(PREDICT_TIME / DT));
            publishTrajectory(trajectory);

            double score = evaluateTrajectory(trajectory, v, w);
            if (score > best_score)
            {
                best_score = score;
                best_v = v;
                best_w = w;
                best_trajectory = trajectory;
            }
        }

        // 5. 发布最优速度指令
        publishVelocity(best_v, best_w);

        // 记录本次指令用于下一周期水流估计
        last_cmd_v_ = best_v;
        last_cmd_w_ = best_w;

        RCLCPP_DEBUG(get_logger(), "Best v: %.2f, w: %.2f, score: %.2f", best_v, best_w, best_score);
    }


    void publishTrajectory(const std::vector<geometry_msgs::msg::Pose>& trajectory)
    {
        nav_msgs::msg::Path path_msg;
        path_msg.header.stamp = this->now();
        path_msg.header.frame_id = "odom";

        for (const auto& pose : trajectory)
        {
            geometry_msgs::msg::PoseStamped stamped_pose;
            stamped_pose.header.stamp = this->now();
            stamped_pose.header.frame_id = "odom";
            stamped_pose.pose = pose;
            path_msg.poses.push_back(stamped_pose);
        }

        forcast_path_pub_->publish(path_msg);
    }

    // 更新当前目标点
    void updateCurrentGoal()
    {
        std::lock_guard<std::mutex> lock(path_mutex_);

        if (current_goal_index_ < 0 || current_goal_index_ >= static_cast<int>(path_poses_.size()))
        {
            RCLCPP_WARN(get_logger(), "Invalid goal index: %d", current_goal_index_);
            return;
        }

        // 获取当前目标点
        const auto &goal_pose = path_poses_[current_goal_index_];
        goal_x_ = goal_pose.position.x;
        goal_y_ = goal_pose.position.y;

        // 发布当前目标点用于可视化
        geometry_msgs::msg::PoseStamped current_goal;
        current_goal.header.stamp = now();
        current_goal.header.frame_id = "odom";
        current_goal.pose = goal_pose;
        goal_pub_->publish(current_goal);

        // 调试输出
        RCLCPP_INFO(get_logger(), "Current goal [%d/%zu]: (%.2f, %.2f)",
                    current_goal_index_ + 1,
                    path_poses_.size(),
                    goal_x_, goal_y_);

        // 检查是否到达当前目标点
        double dx = goal_x_ - current_x_;
        double dy = goal_y_ - current_y_;
        double dist = sqrt(dx * dx + dy * dy);

        RCLCPP_INFO(get_logger(), "Distance to goal: %.2f (tolerance: %.2f)", dist, GOAL_TOLERANCE);

        if (dist < GOAL_TOLERANCE)
        {
            // 移动到下一个点
            current_goal_index_++;
            RCLCPP_INFO(get_logger(), "Reached point, moving to next goal (%d)", current_goal_index_);

            // 检查是否完成所有点
            if (current_goal_index_ >= static_cast<int>(path_poses_.size()))
            {
                RCLCPP_INFO(get_logger(), "All path points reached!");
                stopRobot();
            }
        }
    }

    // 计算前瞻点
    int getLookaheadGoalIndex()
    {
        std::lock_guard<std::mutex> lock(path_mutex_);

        // 找到路径上最接近的点
        int closest_index = current_goal_index_;
        double min_dist = 1e9;

        for (int i = current_goal_index_; i < static_cast<int>(path_poses_.size()); ++i)
        {
            const auto &pose = path_poses_[i];
            double dx = pose.position.x - current_x_;
            double dy = pose.position.y - current_y_;
            double dist = sqrt(dx * dx + dy * dy);

            if (dist < min_dist)
            {
                min_dist = dist;
                closest_index = i;
            }
        }

        // 从最近点开始寻找前瞻点
        double accumulated_dist = 0.0;
        int lookahead_index = closest_index;

        for (int i = closest_index; i < static_cast<int>(path_poses_.size()) - 1; ++i)
        {
            const auto &p1 = path_poses_[i];
            const auto &p2 = path_poses_[i + 1];

            double dx = p2.position.x - p1.position.x;
            double dy = p2.position.y - p1.position.y;
            double segment_dist = sqrt(dx * dx + dy * dy);

            if (accumulated_dist + segment_dist > LOOKAHEAD_DIST)
            {
                // 找到前瞻点
                lookahead_index = i;
                break;
            }

            accumulated_dist += segment_dist;
            lookahead_index = i + 1;
        }

        return lookahead_index;
    }

    // 估算水流速度
    void estimateFlow()
    {
        static double last_x = current_x_;
        static double last_y = current_y_;
        static rclcpp::Time last_time = now();

        double dt = (now() - last_time).seconds();
        if (dt < 0.01)
            dt = DT; // 防止除零

        // 预期位移（基于上一次指令）
        double expected_dx = last_cmd_v_ * cos(current_yaw_) * dt;
        double expected_dy = last_cmd_v_ * sin(current_yaw_) * dt;

        // 实际位移
        double actual_dx = current_x_ - last_x;
        double actual_dy = current_y_ - last_y;

        // 计算水流速度
        flow_x_ = (actual_dx - expected_dx) / dt;
        flow_y_ = (actual_dy - expected_dy) / dt;

        // 应用低通滤波 (减少噪声影响)
        flow_x_ = 0.7 * flow_x_ + 0.3 * flow_x_prev_;
        flow_y_ = 0.7 * flow_y_ + 0.3 * flow_y_prev_;

        // 更新历史值
        flow_x_prev_ = flow_x_;
        flow_y_prev_ = flow_y_;
        last_x = current_x_;
        last_y = current_y_;
        last_time = now();

        RCLCPP_DEBUG(get_logger(), "Flow estimate: (%.2f, %.2f) m/s", flow_x_, flow_y_);
    }

    // 生成动态窗口
    void generateDynamicWindow(double curr_v, double curr_w,
                               double &v_min, double &v_max,
                               double &w_min, double &w_max)
    {
        // 速度限制 - 设置最小前进速度
        double vs_min = 0.1; // 关键修改：最小线速度0.1m/s
        double vs_max = MAX_LINEAR_SPEED;
        double ws_min = -MAX_ANGULAR_SPEED;
        double ws_max = MAX_ANGULAR_SPEED;

        // 加速度限制
        double v_acc_min = curr_v - MAX_LINEAR_ACCEL * DT;
        double v_acc_max = curr_v + MAX_LINEAR_ACCEL * DT;
        double w_acc_min = curr_w - MAX_ANGULAR_ACCEL * DT;
        double w_acc_max = curr_w + MAX_ANGULAR_ACCEL * DT;

        // 最终的动态窗口取交集
        v_min = std::max(vs_min, v_acc_min);
        v_max = std::min(vs_max, v_acc_max);
        w_min = std::max(ws_min, w_acc_min);
        w_max = std::min(ws_max, w_acc_max);

        RCLCPP_DEBUG(get_logger(), "Dyn window: v[%.2f, %.2f], w[%.2f, %.2f]",
                     v_min, v_max, w_min, w_max);
    }

    // 模拟轨迹
    std::vector<geometry_msgs::msg::Pose> simulateTrajectory(double v, double w,
                                                             double dt, int steps)
    {
        std::vector<geometry_msgs::msg::Pose> trajectory;
        double x = current_x_;
        double y = current_y_;
        double theta = current_yaw_;

        for (int i = 0; i < steps; ++i)
        {
            // 船体自身运动（相对水流）
            double dx = v * cos(theta) * dt;
            double dy = v * sin(theta) * dt;
            double dtheta = w * dt;

            // 叠加水流运动
            dx += flow_x_ * dt;
            dy += flow_y_ * dt;

            // 更新位置和航向
            x += dx;
            y += dy;
            theta += dtheta;

            // 添加到轨迹
            geometry_msgs::msg::Pose pose;
            pose.position.x = x;
            pose.position.y = y;

            // 姿态（只关心航向）
            tf2::Quaternion q;
            q.setRPY(0, 0, theta);
            pose.orientation = tf2::toMsg(q);

            trajectory.push_back(pose);
        }

        return trajectory;
    }

    // 评估轨迹
    double evaluateTrajectory(const std::vector<geometry_msgs::msg::Pose> &trajectory,
                              double v, double w)
    {
        if (trajectory.empty())
        {
            return -1e9; // 无效轨迹
        }

        // 1. 目标朝向得分
        const auto &end_pose = trajectory.back();
        double dx = goal_x_ - end_pose.position.x;
        double dy = goal_y_ - end_pose.position.y;
        double target_angle = atan2(dy, dx);

        double end_yaw = tf2::getYaw(end_pose.orientation);
        double angle_diff = fabs(atan2(sin(target_angle - end_yaw),
                                       cos(target_angle - end_yaw)));
        double heading_score = (M_PI - angle_diff) / M_PI; // 归一化[0,1]

        // 2. 速度得分
        double velocity_score = v / MAX_LINEAR_SPEED;

        // 3. 安全间隙得分 - 关键修改：连续函数
        double min_dist = computeMinObstacleDistance(trajectory);
        double safe_threshold = ROBOT_RADIUS + OBSTACLE_INFLATION;
        double clearance_score = 1.0 - exp(-min_dist / (safe_threshold * 2.0));

        // 4. 水流鲁棒性得分
        double flow_robustness = computeFlowRobustness(trajectory, v, w);

        // 加权总分
        double total_score = ALPHA * heading_score +
                             BETA * velocity_score +
                             GAMMA * clearance_score +
                             DELTA * flow_robustness;

        // 调试输出
        // RCLCPP_INFO(get_logger(),
        //             "Scores: H=%.2f(%.2f) V=%.2f(%.2f) C=%.2f(%.2f) F=%.2f T=%.2f",
        //             heading_score, ALPHA * heading_score,
        //             velocity_score, BETA * velocity_score,
        //             clearance_score, GAMMA * clearance_score,
        //             flow_robustness, total_score);

        return total_score;
    }

    // 计算轨迹到障碍物的最小距离
    double computeMinObstacleDistance(const std::vector<geometry_msgs::msg::Pose> &trajectory)
    {
        std::lock_guard<std::mutex> lock(obs_mutex_);

        if (obstacles_.empty())
        {
            return 10.0; // 无障碍物时返回大距离
        }

        double min_dist = 1e9;

        for (const auto &pose : trajectory)
        {
            for (const auto &obs : obstacles_)
            {
                double dx = pose.position.x - obs.first;
                double dy = pose.position.y - obs.second;
                double dist = sqrt(dx * dx + dy * dy);

                // 减去膨胀半径
                dist -= OBSTACLE_INFLATION;

                if (dist < min_dist)
                {
                    min_dist = dist;
                }
            }
        }

        return min_dist;
    }

    // 计算水流鲁棒性
    double computeFlowRobustness(const std::vector<geometry_msgs::msg::Pose> &trajectory,
                                 double v, double w)
    {
        if (trajectory.size() < 2)
            return 0.0;

        // 计算预期终点（无水流）
        double expected_x = current_x_ + v * cos(current_yaw_) * PREDICT_TIME;
        double expected_y = current_y_ + v * sin(current_yaw_) * PREDICT_TIME;

        // 实际终点
        const auto &end_pose = trajectory.back();
        double actual_x = end_pose.position.x;
        double actual_y = end_pose.position.y;

        // 计算位置误差
        double error = sqrt(pow(actual_x - expected_x, 2) + pow(actual_y - expected_y, 2));

        // 误差越小得分越高
        return 1.0 / (1.0 + error);
    }

    // 停止船舶
    void stopRobot()
    {
        auto cmd_vel = geometry_msgs::msg::Twist();
        cmd_vel_pub_->publish(cmd_vel);
        path_poses_.clear();
        RCLCPP_INFO(get_logger(), "Robot stopped.");
    }

    // 发布速度指令
    void publishVelocity(double v, double w)
    {
        auto cmd_vel = geometry_msgs::msg::Twist();
        cmd_vel.linear.x = v;
        cmd_vel.angular.z = w;
        cmd_vel_pub_->publish(cmd_vel);
    }

    // 状态变量
    double current_x_ = 0.0;
    double current_y_ = 0.0;
    double current_yaw_ = 0.0;
    double current_v_ = 0.0;
    double current_vx_ = 0.0;
    double current_vy_ = 0.0;
    double current_w_ = 0.0;
    double goal_x_ = 0.0;
    double goal_y_ = 0.0;

    // 水流估计
    double flow_x_ = 0.0;
    double flow_y_ = 0.0;
    double flow_x_prev_ = 0.0;
    double flow_y_prev_ = 0.0;

    // 上一次指令
    double last_cmd_v_ = 0.0;
    double last_cmd_w_ = 0.0;

    // 路径相关
    std::vector<geometry_msgs::msg::Pose> path_poses_;
    int current_goal_index_ = -1;
    bool path_received_ = false;
    std::mutex path_mutex_;

    // 障碍物列表
    std::vector<std::pair<double, double>> obstacles_;
    std::mutex obs_mutex_;

    // ROS接口
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr radar_sub_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr forcast_path_pub_;

};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<DWAPlanner>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}